/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package GUI;

import dynamixel.Actuator;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.util.ArrayList;
import java.util.StringTokenizer;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.swing.ListModel;
import javax.swing.Timer;
import javax.swing.event.ListDataEvent;
import javax.swing.event.ListDataListener;

/**
 *
 * @author minaev
 */
public class LearnPositionActuator implements ListModel<String> {

    ArrayList<Integer> actuator_nums = new ArrayList<>();
    ArrayList<Integer> actuator_pos = new ArrayList<>();
    ArrayList<Integer> actuator_speed = new ArrayList<>();
    ArrayList<Integer> actuator_tor = new ArrayList<>();
    ArrayList<Integer> actuator_scan = new ArrayList<>();
    ArrayList<ListDataListener> ldl = new ArrayList<>();
    Timer scanPos = new Timer(500, new ActionListener() {
        @Override
        public void actionPerformed(ActionEvent e) {
            buf = 0;
            for (int i = 0; i < old_pos.length; i++) {
                do {
                    cur_pos = actuators[i].getCurPos();
                    try {
                        Thread.sleep(10);
                    } catch (InterruptedException ex) {
                        Logger.getLogger(LearnPositionActuator.class.getName()).log(Level.SEVERE, null, ex);
                    }
                } while (cur_pos == -1);

                err_pos[i] = Math.abs(old_pos[i] - cur_pos);
                //System.out.println("Error "+i+": "+err_pos[i]);
                if (err_pos[i] > err_pos[buf]) {
                    buf = i;
                }
            }
            num_max_dpos = buf;
        }
    });
    int old_pos[];
    int err_pos[];
    int buf = 0;
    int cur_pos = 0;
    int num_max_dpos = -1;
    Actuator actuators[];
    int def_speed = 255;
    int def_tor = 255;
    boolean def_scan = false;
    int def_chn = 0;

    public LearnPositionActuator(Actuator actuators[]) {
        this.actuators = actuators;
        old_pos = new int[actuators.length];
        err_pos = new int[actuators.length];
        updateOldPos();
        scanPos.start();
    }

    private void updateOldPos() {
        for (int i = 0; i < old_pos.length; i++) {
            do {
                old_pos[i] = actuators[i].getCurPos();
                try {
                    Thread.sleep(10);
                } catch (InterruptedException ex) {
                    Logger.getLogger(LearnPositionActuator.class.getName()).log(Level.SEVERE, null, ex);
                }
                //System.out.println("Update old pos "+i);
            } while (old_pos[i] == -1);
        }
    }

    public void torqueDisable() {
        for (int i = 0; i < actuators.length; i++) {
            actuators[i].setTorqueEnable(false);
        }
    }

    public void torqueDisable(int num_act) {
        actuators[num_act].setTorqueEnable(false);
    }

    public void clearLearn() {
        actuator_nums.clear();
        actuator_pos.clear();
    }

    public int learn(int num_actuator, int beforeLine) {
        int pos = actuators[num_actuator].getCurPos();
        int cnt = 5;
        while (cnt > 0 && pos == -1) {
            pos = actuators[num_actuator].getCurPos();
        }
        if (cnt != 0) {
            if (beforeLine >= 0 && beforeLine < getSize()) {
                actuator_pos.add(beforeLine+1,pos);
                actuator_nums.add(beforeLine+1,num_actuator);
                actuator_speed.add(beforeLine+1,def_speed);
                actuator_tor.add(beforeLine+1,def_tor);
                actuator_scan.add(beforeLine+1,def_chn);
            }else{
                actuator_pos.add(pos);
                actuator_nums.add(num_actuator);
                actuator_speed.add(def_speed);
                actuator_tor.add(def_tor);
                actuator_scan.add(def_chn);
            }
            if (def_scan) {
                def_chn = 0;
                def_scan = false;
            }
            updateOldPos();
            change();
            return pos;
        } else {
            System.out.println("Learn filed! pos: " + pos);
            return -1;
        }
    }

    public void setSpeed(int speed) {
        def_speed = speed;
    }

    public void setTor(int tor) {
        def_tor = tor;
    }

    public void setScaning(int chnum) {
        def_chn = chnum;
        def_scan = (chnum == 0);
    }

    public String getScript() {
        String res = "";
        for (int i = 0; i < actuator_nums.size(); i++) {
            res += this.getElementAt(i);
        }
        return res;
    }

    public void loadScript(String script) {
        this.removeAll();
        StringTokenizer st = new StringTokenizer(script, "" + (char) 13 + (char) 10, false);
        while (st.countTokens() > 0) {
            addScriptLine(st.nextToken());
        }
    }

    public void addScriptLine(String scr_line) {
        StringTokenizer st = new StringTokenizer(scr_line, "\t ;", false);
        if (st.countTokens() >= 5) {
            actuator_nums.add(new Integer(st.nextToken()));
            actuator_pos.add(new Integer(st.nextToken()));
            actuator_speed.add(new Integer(st.nextToken()));
            actuator_tor.add(new Integer(st.nextToken()));
            actuator_scan.add(new Integer(st.nextToken()));
        }
    }

    public int getSpeed() {
        return def_speed;
    }

    public int getTor() {
        return def_tor;
    }

    public boolean isScan() {
        return def_scan;
    }

    public int getNumMaxDpos() {
        return num_max_dpos;
    }

    @Override
    public int getSize() {
        return actuator_nums.size();
    }

    @Override
    public String getElementAt(int index) {
        return actuator_nums.get(index) + ";" + actuator_pos.get(index) + ";"
                + actuator_speed.get(index) + ";" + actuator_tor.get(index) + ";"
                + actuator_scan.get(index) + ";\n";
    }

    @Override
    public void addListDataListener(ListDataListener l) {
        ldl.add(l);
    }

    @Override
    public void removeListDataListener(ListDataListener l) {
        ldl.remove(l);
    }

    public void remove(int i) {
        actuator_nums.remove(i);
        actuator_pos.remove(i);
        actuator_speed.remove(i);
        actuator_tor.remove(i);
        actuator_scan.remove(i);
        change();
    }

    public void removeAll() {
        actuator_nums.clear();
        actuator_pos.clear();
        actuator_speed.clear();
        actuator_tor.clear();
        actuator_scan.clear();
        change();
    }

    private void change() {
        for (int i = 0; i < ldl.size(); i++) {
            ldl.get(i).contentsChanged(new ListDataEvent(this,
                    ListDataEvent.CONTENTS_CHANGED, 0, ldl.size() - 1));
        }
    }

    public ArrayList<Integer> getActuatorNums() {
        return actuator_nums;
    }

    public ArrayList<Integer> getActuatorPos() {
        return actuator_pos;
    }

    public ArrayList<Integer> getActuatorSpeed() {
        return actuator_speed;
    }

    public ArrayList<Integer> getActuatorTor() {
        return actuator_tor;
    }

    public ArrayList<Integer> getActuatorScan() {
        return actuator_scan;
    }

    public Actuator[] getActuators() {
        return actuators;
    }
}
